#pragma once

#include <cassert>

#include "zakengine/zak.h"
#include "misc/vector2D.h"
#include "misc/c2Dmatrix.h"

using namespace zak;

class MovingEntity : public Sprite
{

public:


	MovingEntity(
					Vector2D	heading,
					Vector2D	velocity,
					double		max_speed,
					double		turn_rate,
					double		max_force,
					double		mass):	m_vHeading(heading),
											m_vVelocity(velocity),
											m_dMaxSpeed(max_speed),
											m_dMaxTurnRate(turn_rate),
											m_dMaxForce(max_force),
											m_dMass(mass),
											m_vSide(m_vHeading.Perp())
	{
		
	}

	virtual ~MovingEntity(){}

	//accessors
	Vector2D	Velocity()const{return m_vVelocity;}
	void		SetVelocity(const Vector2D& NewVel){m_vVelocity = NewVel;}

	double     Mass()const{return m_dMass;}

	Vector2D	Side()const{return m_vSide;}

	double     MaxSpeed()const{return m_dMaxSpeed;}                       
	void		SetMaxSpeed(double new_speed){m_dMaxSpeed = new_speed;}

	double     MaxForce()const{return m_dMaxForce;}
	void		SetMaxForce(double mf){m_dMaxForce = mf;}

	bool		IsSpeedMaxedOut()const{return m_dMaxSpeed*m_dMaxSpeed >= m_vVelocity.LengthSq();}
	double     Speed()const{return m_vVelocity.Length();}
	double     SpeedSq()const{return m_vVelocity.LengthSq();}

	Vector2D	Heading()const{return m_vHeading;}
	void		SetHeading(Vector2D new_heading);
	bool		RotateHeadingToFacePosition(Vector2D target);

	double     MaxTurnRate()const{return m_dMaxTurnRate;}
	void		SetMaxTurnRate(double val){m_dMaxTurnRate = val;}


protected:
  
  Vector2D    m_vVelocity;
  
  // A normalized vector pointing in the direction the entity is heading. 
  Vector2D    m_vHeading;

  // A vector perpendicular to the heading vector
  Vector2D    m_vSide; 

  double       m_dMass;
  
  // The maximum speed this entity may travel at.
  double       m_dMaxSpeed;

  // The maximum force this entity can produce to power itself 
  double        m_dMaxForce;
  
  //the maximum rate (radians per second)this vehicle can rotate         
  double       m_dMaxTurnRate;
};


//--------------------------- RotateHeadingToFacePosition ---------------------
//
//  given a target position, this method rotates the entity's heading and
//  side vectors by an amount not greater than m_dMaxTurnRate until it
//  directly faces the target.
//
//  returns true when the heading is facing in the desired direction
//-----------------------------------------------------------------------------
inline bool MovingEntity::RotateHeadingToFacePosition(Vector2D target)
{
  Vector2D toTarget = Vec2DNormalize(target - Vector2D(GetPosX(),GetPosY()));

  //first determine the angle between the heading vector and the target
  double angle = acos(m_vHeading.Dot(toTarget));

  //return true if the player is facing the target
  if (angle < 0.00001) return true;

  //clamp the amount to turn to the max turn rate
  if (angle > m_dMaxTurnRate) angle = m_dMaxTurnRate;
  
  //The next few lines use a rotation matrix to rotate the player's heading
  //vector accordingly
  C2DMatrix RotationMatrix;
  
  //notice how the direction of rotation has to be determined when creating
  //the rotation matrix

  RotationMatrix.Rotate(angle * m_vHeading.Sign(toTarget));	
  RotationMatrix.TransformVector2Ds(m_vHeading);
  RotationMatrix.TransformVector2Ds(m_vVelocity);

  //finally recreate m_vSide
  m_vSide = m_vHeading.Perp();

  return false;
}


//------------------------- SetHeading ----------------------------------------
//
//  first checks that the given heading is not a vector of zero length. If the
//  new heading is valid this fumction sets the entity's heading and side 
//  vectors accordingly
//-----------------------------------------------------------------------------
inline void MovingEntity::SetHeading(Vector2D new_heading)
{
  assert( (new_heading.LengthSq() - 1.0) < 0.00001);
  
  m_vHeading = new_heading;

  //the side vector must always be perpendicular to the heading
  m_vSide = m_vHeading.Perp();
}